% 参数设置
m = 100;
n = 50;
k = 30;
num_trials = 5;  % 减少试验次数（病态矩阵计算更耗时）
cond_numbers = [1e2, 1e6, 1e10, 1e14]; % 测试不同条件数

% 预分配结果存储
results = cell(length(cond_numbers), 1);

fprintf('===== 伪逆算法在病态矩阵上的性能测试 =====\n');
fprintf('矩阵大小: %d×%d, 系数维度: %d×%d\n', m, n, n, k);
fprintf('试验次数: %d\n', num_trials);

for cidx = 1:length(cond_numbers)
    cond_num = cond_numbers(cidx);
    fprintf('\n===== 条件数: %.0e =====\n', cond_num);
    
    % 当前条件数的结果存储
    times_qril = zeros(num_trials, 1);
    times_pinv = zeros(num_trials, 1);
    errors_qril = zeros(num_trials, 1);
    errors_pinv = zeros(num_trials, 1);
    diff_pinv = zeros(num_trials, 1);
    cond_actual = zeros(num_trials, 1);
    
    fprintf('--------------------------------------------\n');
    fprintf('试验\t实际条件数\tQRIL时间(ms)\tpinv时间(ms)\tQRIL误差\tpinv误差\t与pinv差异\n');
    fprintf('--------------------------------------------\n');
    
    for trial = 1:num_trials
        % 生成病态矩阵 (使用不同方法)
        if mod(trial, 2) == 0
            % 方法1: 使用gallery创建Hilbert类矩阵
            A = gallery('randhess', max(m, n));
            A = A(1:m, 1:n);
        else
            % 方法2: 使用SVD构造指定条件数矩阵
            [U, ~, V] = svd(randn(m, n), 'econ');
            s = logspace(0, log10(1/cond_num), min(m, n));
            A = U * diag(s) * V';
        end
        
        % 记录实际条件数
        cond_actual(trial) = cond(A);
        
        beta_std = rand(n, k);
        T_true = A * beta_std;
        d = min(m, n);
        T = eye(d);
        
        % 测试QRIL算法
        tic;
        if m > n
            Beta_qril = QRIL_inverse.QR_schmidt(A', T);
            Beta_qril = Beta_qril';
        else
            Beta_qril = QRIL_inverse.QR_schmidt(A, T);
        end
        times_qril(trial) = toc * 1000;
        
        % 测试MATLAB pinv
        tic;
        Beta_pinv = pinv(A);
        times_pinv(trial) = toc * 1000;
        
        % 计算误差
        beta_recon_qril = Beta_qril * T_true;
        beta_recon_pinv = Beta_pinv * T_true;
        
        errors_qril(trial) = norm(beta_recon_qril - beta_std, "fro");
        errors_pinv(trial) = norm(beta_recon_pinv - beta_std, "fro");
        diff_pinv(trial) = norm(Beta_qril - Beta_pinv, "fro") / norm(Beta_pinv, "fro");
        
        % 输出当前试验结果
        fprintf('%d\t%.2e\t%.2f\t\t%.2f\t\t%.2e\t%.2e\t%.2e\n', ...
                trial, cond_actual(trial), times_qril(trial), times_pinv(trial), ...
                errors_qril(trial), errors_pinv(trial), diff_pinv(trial));
    end
    
    % 存储当前条件数下的结果
    results{cidx} = struct(...
        'cond_num', cond_num, ...
        'times_qril', times_qril, ...
        'times_pinv', times_pinv, ...
        'errors_qril', errors_qril, ...
        'errors_pinv', errors_pinv, ...
        'diff_pinv', diff_pinv, ...
        'cond_actual', cond_actual);
end

%% 结果可视化
figure('Position', [100, 100, 1200, 800]);

% 时间对比
subplot(2, 2, 1);
hold on;
for cidx = 1:length(cond_numbers)
    avg_time_qril = mean(results{cidx}.times_qril);
    avg_time_pinv = mean(results{cidx}.times_pinv);
    plot(cond_numbers(cidx), avg_time_qril, 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    plot(cond_numbers(cidx), avg_time_pinv, 'rs', 'MarkerSize', 10, 'LineWidth', 2);
end
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'log');
xlabel('条件数');
ylabel('平均计算时间 (ms)');
title('计算时间 vs 条件数');
legend('QRIL', 'pinv', 'Location', 'northwest');
grid on;

% 误差对比
subplot(2, 2, 2);
hold on;
for cidx = 1:length(cond_numbers)
    avg_error_qril = mean(results{cidx}.errors_qril);
    avg_error_pinv = mean(results{cidx}.errors_pinv);
    plot(cond_numbers(cidx), avg_error_qril, 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    plot(cond_numbers(cidx), avg_error_pinv, 'rs', 'MarkerSize', 10, 'LineWidth', 2);
end
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'log');
xlabel('条件数');
ylabel('平均重建误差 (Fro)');
title('重建误差 vs 条件数');
grid on;

% 算法差异
subplot(2, 2, 3);
hold on;
for cidx = 1:length(cond_numbers)
    avg_diff = mean(results{cidx}.diff_pinv);
    plot(cond_numbers(cidx), avg_diff, 'md', 'MarkerSize', 10, 'LineWidth', 2);
end
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'log');
xlabel('条件数');
ylabel('平均相对差异 (Fro)');
title('与pinv的相对差异 vs 条件数');
grid on;

% 条件数分布
subplot(2, 2, 4);
% 提取所有数据并转换为列向量
data = cell2mat(cellfun(@(x) x.cond_actual(:), results, 'UniformOutput', false));

% 创建分组变量 g（与 data 长度相同）
% 例如：如果 results 有 3 组，每组 10 个数据点，则 g = [1,1,...,1,2,2,...,2,3,3,...,3]'
group_lengths = cellfun(@(x) numel(x.cond_actual), results); % 每组数据长度
g = arrayfun(@(group_idx, len) group_idx * ones(len, 1), ...
            1:numel(results), group_lengths(:)', 'UniformOutput', false);
g = vertcat(g{:});

% 生成标签（数量必须等于分组数，即 numel(results)）
labels = arrayfun(@(x) sprintf('%.0e', x), cond_numbers, 'UniformOutput', false);

% 使用分组变量 g 绘制箱线图
boxplot(data, g, 'Labels', labels);
title('实际条件数分布');
ylabel('条件数');
set(gca, 'YScale', 'log');
grid on;

%% 病态矩阵特性可视化
figure('Position', [100, 100, 1200, 500]);
for cidx = 1:min(4, length(cond_numbers))
    % 生成一个代表性病态矩阵
    [U, ~, V] = svd(randn(m, n), 'econ'); % 修正：使用完整秩
    s = logspace(0, log10(1/cond_numbers(cidx)), min(m, n));
    A_vis = U(:, 1:min(m,n)) * diag(s) * V(:, 1:min(m,n))'; % 现在不会越界了
    
    subplot(2, 4, cidx);
    imagesc(A_vis);
    title(sprintf('Condition No.: %.0e', cond_numbers(cidx)));
    colorbar;
    
    subplot(2, 4, cidx+4);
    semilogy(s, 'LineWidth', 2);
    title('Singular Value Distribution');
    xlabel('Index');
    ylabel('Singular Value');
    grid on;
end

%% 伪逆结果对比（选择最高条件数的试验）
high_cond_result = results{end};
trial_idx = find(high_cond_result.cond_actual == max(high_cond_result.cond_actual), 1);

% 重新计算以获取数据
[U, ~, V] = svd(randn(m, n), 'econ');
s = logspace(0, log10(1/cond_numbers(end)), min(m, n));
A_high = U * diag(s) * V';
beta_std = rand(n, k);
T_true = A_high * beta_std;
d = min(m, n);
T = eye(d);

if m > n
    Beta_qril = QRIL_inverse.QR_schmidt(A_high', T);
    Beta_qril = Beta_qril';
else
    Beta_qril = QRIL_inverse.QR_schmidt(A_high, T);
end
Beta_pinv = pinv(A_high);

% 可视化伪逆结果
figure('Position', [100, 100, 1400, 500]);
subplot(1, 3, 1);
imagesc(Beta_pinv);
title('MATLAB pinv结果 (高病态)');
colorbar;

subplot(1, 3, 2);
imagesc(Beta_qril);
title('QRIL算法结果 (高病态)');
colorbar;

subplot(1, 3, 3);
imagesc(abs(Beta_qril - Beta_pinv));
title('差异绝对值');
colorbar;

% 重建系数对比
beta_recon_qril = Beta_qril * T_true;
beta_recon_pinv = Beta_pinv * T_true;

figure('Position', [100, 100, 1400, 500]);
subplot(1, 3, 1);
imagesc(beta_std);
title('原始系数');
colorbar;

subplot(1, 3, 2);
imagesc(beta_recon_qril);
title('QRIL重建系数');
colorbar;
subtitle(sprintf('误差: %.2e', norm(beta_recon_qril - beta_std, "fro")));

subplot(1, 3, 3);
imagesc(beta_recon_pinv);
title('pinv重建系数');
colorbar;
subtitle(sprintf('误差: %.2e', norm(beta_recon_pinv - beta_std, "fro")));